#include "main.h"

static CombinedApi capi = CombinedApi();
static bool apiSupportsBX2 = false;



/**
 * @brief There's no standard cross platform sleep() method prior to C++11
 */
void sleepSeconds(unsigned numSeconds)
{
#ifdef _WIN32
	Sleep((DWORD)1000 * numSeconds); // Sleep(ms)
#else
	sleep(numSeconds); // sleep(sec)
#endif
}

/**
 * @brief Prints a debug message if a method call failed.
 * @details To use, pass the method name and the error code returned by the method.
 *          Eg: onErrorPrintDebugMessage("capi.initialize()", capi.initialize());
 *          If the call succeeds, this method does nothing.
 *          If the call fails, this method prints an error message to stdout.
 */
void onErrorPrintDebugMessage(std::string methodName, int errorCode)
{
	if (errorCode < 0)
	{
		std::cout << methodName << " failed: " << capi.errorToString(errorCode) << std::endl;
	}
}

/**
* @brief Returns the string: "[tool.id] s/n:[tool.serialNumber]" used in CSV output
*/
std::string getToolInfo(std::string toolHandle)
{
	// Get the port handle info from PHINF
	PortHandleInfo info = capi.portHandleInfo(toolHandle);

	// Return the ID and SerialNumber the desired string format
	std::string outputString = info.getToolId();
	outputString.append(" s/n:").append(info.getSerialNumber());
	return outputString;
}

/**
* @brief Returns a string representation of the data in CSV format.
* @details The CSV format is: "Frame#,ToolHandle,Face,TransformStatus,q0,qx,qy,qz,tx,ty,tz,error,#markers,[Marker1:status,x,y,z],[Marker2..."
*/
std::string toolDataToCSV(const ToolData& toolData)
{
	std::stringstream stream;
	stream << std::setprecision(toolData.PRECISION) << std::setfill('0');
	stream << "" << static_cast<unsigned>(toolData.frameNumber) << ","
		   << "Port:" << static_cast<unsigned>(toolData.transform.toolHandle) << ",";
	stream << static_cast<unsigned>(toolData.transform.getFaceNumber()) << ",";

	if (toolData.transform.isMissing())
	{
		stream << "Missing,,,,,,,,";
	}
	else
	{
		stream << TransformStatus::toString(toolData.transform.getErrorCode()) << ","
			   << toolData.transform.q0 << "," << toolData.transform.qx << "," << toolData.transform.qy << "," << toolData.transform.qz << ","
			   << toolData.transform.tx << "," << toolData.transform.ty << "," << toolData.transform.tz << "," << toolData.transform.error;
	}

	// Each marker is printed as: status,tx,ty,tz
	stream << "," << toolData.markers.size();
	for ( int i = 0; i < toolData.markers.size(); i++)
	{
		stream << "," << MarkerStatus::toString(toolData.markers[i].status);
		if (toolData.markers[i].status == MarkerStatus::Missing)
		{
			stream << ",,,";
		}
		else
		{
			stream << "," << toolData.markers[i].x << "," << toolData.markers[i].y << "," << toolData.markers[i].z;
		}
	}
	return stream.str();
}


/**
 * @brief Prints a ToolData object to stdout
 * @param toolData The data to print
 */
void printToolData(const ToolData& toolData)
{
	if (toolData.systemAlerts.size() > 0)
	{
		std::cout << "[" << toolData.systemAlerts.size() << " alerts] ";
		for (int a = 0; a < toolData.systemAlerts.size(); a++)
		{
			std::cout << toolData.systemAlerts[a].toString() << std::endl;
		}
	}

	if (toolData.buttons.size() > 0)
	{
		std::cout << "[buttons: ";
		for (int b = 0; b < toolData.buttons.size(); b++)
		{
			std::cout << ButtonState::toString(toolData.buttons[b]) << " ";
		}
		std::cout << "] ";
	}
	std::cout << toolDataToCSV(toolData) << std::endl;
}

/**
 * @brief Put the system into tracking mode, and get a few frames of data.
 */
void printTrackingData()
{
	// Start tracking, output a few frames of data, and stop tracking
	std::cout << std::endl << "Entering tracking mode and collecting data..." << std::endl;
	onErrorPrintDebugMessage("capi.startTracking()", capi.startTracking());
	for (int i = 0; i < 10; i++)
	{
		// Demonstrate TX command: ASCII command sent, ASCII reply received
		std::cout << capi.getTrackingDataTX() << std::endl;

		// Demonstrate BX or BX2 command
		std::vector<ToolData> toolData =  apiSupportsBX2 ? capi.getTrackingDataBX2() : capi.getTrackingDataBX();

		// Print to stdout in similar format to CSV
		std::cout << "[alerts] [buttons] Frame#,ToolHandle,Face#,TransformStatus,Q0,Qx,Qy,Qz,Tx,Ty,Tz,Error,#Markers,State,Tx,Ty,Tz" << std::endl;
		for (int i = 0; i < toolData.size(); i++)
		{
			printToolData(toolData[i]);
		}
	}

	// Stop tracking (back to configuration mode)
	std::cout << std::endl << "Leaving tracking mode and returning to configuration mode..." << std::endl;
	onErrorPrintDebugMessage("capi.stopTracking()", capi.stopTracking());
}

/**
 * @brief Initialize and enable loaded tools. This is the same regardless of tool type.
 */
void initializeAndEnableTools()
{
	std::cout << std::endl << "Initializing and enabling tools..." << std::endl;

	// Initialize and enable tools
	std::vector<PortHandleInfo> portHandles = capi.portHandleSearchRequest(PortHandleSearchRequestOption::NotInit);
	for (int i = 0; i < portHandles.size(); i++)
	{
		onErrorPrintDebugMessage("capi.portHandleInitialize()", capi.portHandleInitialize(portHandles[i].getPortHandle()));
		onErrorPrintDebugMessage("capi.portHandleEnable()", capi.portHandleEnable(portHandles[i].getPortHandle()));
	}

	// Print all enabled tools
	portHandles = capi.portHandleSearchRequest(PortHandleSearchRequestOption::Enabled);
	for (int i = 0; i < portHandles.size(); i++)
	{
		std::cout << portHandles[i].toString() << std::endl;
	}
}

/**
 * @brief Loads a tool from a tool definition file (.rom)
 */
void loadTool(const char* toolDefinitionFilePath)
{
	// Request a port handle to load a passive tool into
	int portHandle = capi.portHandleRequest();
	onErrorPrintDebugMessage("capi.portHandleRequest()", portHandle);

	// Load the .rom file using the previously obtained port handle
	capi.loadSromToPort(toolDefinitionFilePath, portHandle);
}

/**
 * @brief Demonstrate loading passive tools.
 * @details Passive tools use NDI spheres to passively reflect IR light to the cameras.
 */
void configurePassiveTools()
{
	// Load a few passive tool definitions from a .rom files
	std::cout << std::endl << "Configuring Passive Tools - Loading .rom Files..." << std::endl;
	loadTool("/home/wusoonvi/Desktop/NDI/CombinedAPIsample/sroms/8700339.rom");
}



/**
 * @brief Determines whether an NDI device supports the BX2 command by looking at the API revision
 */
void determineApiSupportForBX2()
{
	// Lookup the API revision
	std::string response = capi.getApiRevision();

	// Refer to the API guide for how to interpret the APIREV response
	char deviceFamily = response[0];
	int majorVersion = capi.stringToInt(response.substr(2,3));

	// As of early 2017, the only NDI device supporting BX2 is the Vega
	// Vega is a Polaris device with API major version 003
	if ( deviceFamily == 'G' && majorVersion >= 3)
	{
		apiSupportsBX2 = true;
	}
}


/**
 * @brief   The entry point for the CAPIsample application.
 * @details The IP_adress  is seted to "192.168.1.110";
 */
int main(int argc, char* argv[])
{
  	ros::init(argc, argv, "ndi_sensor_node");
 	ros::NodeHandle nh;

  	//定义发布的频率 
 	ros::Rate loop_rate(100);


	std::string IP_adress ="192.168.1.110";
	std::string hostname = std::string(IP_adress);
	std::string scu_hostname = "";

	// Attempt to connect to the device
	if (capi.connect(hostname) != 0)
	{
		// Print the error and exit if we can't connect to a device
		std::cout << "Connection Failed!" << std::endl;
		std::cout << "Press Enter to continue...";
		std::cin.ignore();
		return -1;
	}
	std::cout << "Connected!" << std::endl;

	// Wait a second - needed to support connecting to LEMO Vega
	sleepSeconds(1);

	// Determine if the connected device supports the BX2 command
	determineApiSupportForBX2();

	// Initialize the system. This clears all previously loaded tools, unsaved settings etc...
	onErrorPrintDebugMessage("capi.initialize()", capi.initialize());

	// Demonstrate error handling by asking for tracking data in the wrong mode
	std::cout << capi.getTrackingDataTX() << std::endl;

	// Various tool types are configured in slightly different ways
	configurePassiveTools();

	// Once loaded or detected, tools are initialized and enabled the same way
	initializeAndEnableTools();


	// Once the system is put into tracking mode, data is returned for whatever tools are enabled
	printTrackingData();

	std::cout << std::endl << "Entering tracking mode and collecting data..." << std::endl;
	onErrorPrintDebugMessage("capi.startTracking()", capi.startTracking());

	//循环发布msg
  while (ros::ok())
  {
	printTrackingData();
  }

	// Stop tracking (back to configuration mode)
	std::cout << std::endl << "Leaving tracking mode and returning to configuration mode..." << std::endl;
	onErrorPrintDebugMessage("capi.stopTracking()", capi.stopTracking());
	// Give the user a chance to view the output in the terminal before exiting
	std::cout << "CAPI demonstration complete." << std::endl;
	return 0;
}